#include "UtilsConvolve.h"


// convolves the src with kernel and puts result in dst
// no in place convolution
void vuConvolve(
    IplImage * src,
    IplImage * dst,
    vuRect window,
    float * kernel,
    int kernelWidth,
    int kernelHeight
    )
{
    assert( src->width == dst->width );
    assert( src->height == dst->height );
    assert( src->widthStep == dst->widthStep );

    float * srcFloat = (float *) src->imageData;
    float * dstFloat = (float *) dst->imageData;
    int srcStep = src->widthStep / 4;
    int middleX = kernelWidth/2;
    int middleY = kernelHeight/2;

    if (window.x < 0) window.x = 0;
    if (window.y < 0) window.y = 0;
    if (window.x+window.width > src->width) window.width = src->width-window.x;
    if (window.y+window.height > src->height) window.height = src->height-window.y;

    // inefficient implementation!
    for (int row = window.y; row < window.y + window.height; row++)
    {
        for (int col = window.x; col < window.x + window.width; col++)
        {
            dstFloat[row * srcStep + col] = 0;

            int kernelOffset = kernelHeight*kernelWidth-1;

            for (int rowK = kernelHeight-1; rowK >= 0; rowK--)
            {
                for (int colK = kernelWidth-1; colK >= 0; colK--)
                {
                    int iX = col - middleX + colK;
                    iX = iX < 0 ? 0 : (iX >= src->width ? src->width-1 : iX);
                    int iY = row - middleY + rowK;
                    iY = iY < 0 ? 0 : (iY >= src->height ? src->height-1 : iY);

                    dstFloat[row * srcStep + col] += srcFloat[iY * srcStep + iX] * kernel[kernelOffset--];
                }
            }
        }
    }
}

void vuConvolve(
    IplImage * src,
    IplImage * dst,
    float * kernel,
    int kernelWidth,
    int kernelHeight
    ) {
    vuConvolve(src, dst, vuRect(0,0,src->width, src->height), kernel, kernelWidth, kernelHeight);
}

// convolves the src with kernel and puts result in dst
// no in place convolution
void vuConvolveHorizontal(
    IplImage * src,
    IplImage * dst,
    vuRect window,
    float * kernel,
    int kernelLength
    )
{
    assert( src->width == dst->width );
    assert( src->height == dst->height );
    assert( src->widthStep == dst->widthStep );

    float * srcFloat = (float *) src->imageData;
    float * dstFloat = (float *) dst->imageData;
    int srcStep = src->widthStep / 4;
    int middleX = kernelLength/2;

    if (window.x < 0) window.x = 0;
    if (window.y < 0) window.y = 0;
    if (window.x+window.width > src->width) window.width = src->width-window.x;
    if (window.y+window.height > src->height) window.height = src->height-window.y;

    for (int row = window.y; row < window.y + window.height; row++)
    {
        for (int col = window.x; col < window.x + window.width; col++)
        {
            dstFloat[row * srcStep + col] = 0;
            int iX = col - middleX;

            for (int k = kernelLength-1; k >= 0; k--)
            {
                int i = iX < 0 ? 0 : (iX >= src->width ? src->width-1 : iX);
                dstFloat[row * srcStep + col] += srcFloat[row * srcStep + i] * kernel[k];
                iX++;
            }
        }
    }
}

// convolves the src with kernel and puts result in dst
// no in place convolution
void vuConvolveVertical(
    IplImage * src,
    IplImage * dst,
    vuRect window,
    float * kernel,
    int kernelLength
    )
{
    assert( src->width == dst->width );
    assert( src->height == dst->height );
    assert( src->widthStep == dst->widthStep );

    float * srcFloat = (float *) src->imageData;
    float * dstFloat = (float *) dst->imageData;
    int srcStep = src->widthStep / 4;
    int middleY = kernelLength/2;

    if (window.x < 0) window.x = 0;
    if (window.y < 0) window.y = 0;
    if (window.x+window.width > src->width) window.width = src->width-window.x;
    if (window.y+window.height > src->height) window.height = src->height-window.y;

    for (int row = window.y; row < window.y + window.height; row++)
    {
        for (int col = window.x; col < window.x + window.width; col++)
        {
            dstFloat[row * srcStep + col] = 0;
            int iY = row - middleY;

            for (int k = kernelLength-1; k >= 0; k--)
            {
                int i = iY < 0 ? 0 : (iY >= src->height ? src->height-1 : iY);
                dstFloat[row * srcStep + col] += srcFloat[i * srcStep + col] * kernel[k];
                iY++;
            }
        }
    }
}
